/**
 * @brief 			NNet.cpp Member Function Implementations
 */

#include "NNet.h"

#include "Config.h"
#include "Types.h"

extern "C" {
  #include "NN/network.h"
}

#include <iostream>

NNet::NNet()
{
  for(int i = 0; i < NUM_SENS; ++i)
  {
    os_2[i] = 0;
    os_1[i] = 0;
    os_0[i] = 0;
  }

  make_nn(NUM_SENS+1, NUM_HIDDEN_UNITS, NUM_OUTPUT_UNITS, NUM_PATTERNS);
  init_units(WEIGHT_MIN, WEIGHT_MAX);
  Weights();
  setWeights();
}

NNet::~NNet()
{
  std::cout << "\n//final weights right:\n";
  for(int i = 0; i < NUM_SENS; ++i)
    std::cout << "std_wr[" << printSensor(static_cast<SensorIndex>(i)) << "].setV( " << weight_right[i].getV() << " ); \n";

  std::cout << "\n//final weights left:\n";
  for(int i = 0; i < NUM_SENS; ++i)
    std::cout << "std_wl[" << printSensor(static_cast<SensorIndex>(i)) << "].setV( " << weight_left[i].getV() << " ); \n";

  destroy_nn();
}

void NNet::input(const Sensors& sensor)
{
  for(int i = 0; i < NUM_SENS; ++i)
  {
    os_2[i] = os_1[i];
    os_1[i] = os_0[i];
    os_0[i] = sensor[i];

    double sum = (os_2[i] + os_1[i] + os_0[i])/3.0;
    if(sum > 500)
    {
      weight_right[i].randomV();
      weight_left[i].randomV();
    }
  }
  setWeights();

  /*for(int t = 0; t < NUM_HIDDEN_UNITS; ++t)
  {*/
    for(int i = 0; i < NUM_SENS; ++i)
    {
      /*set_link(WEIGHT, LAYER_HIDDEN, 0, LAYER_IN, 0, sensor[i]);
      set_link(WEIGHT, LAYER_HIDDEN, t, LAYER_IN, i, sensor[i]);*/
      set_pattern(0 , LAYER_IN, i, sensor[i]);
      set_unit(ACTIVATION, LAYER_IN, i, 1 );
      /*for(int j = 0; j < NUM_PATTERNS; ++j)
      {
        set_pattern(j , LAYER_IN, i, sensor[i]);
        set_pattern(j , LAYER_OUT, 0, sensor[i]);
        set_pattern(j , LAYER_OUT, 1, sensor[i]);
      }*/
//    }
  }
}

void NNet::setWeights()
{
	set_link(WEIGHT, LAYER_HIDDEN, 0, LAYER_IN, RIGHT_FRONT, weight_left[RIGHT_FRONT].getV());
	set_link(WEIGHT, LAYER_HIDDEN, 0, LAYER_IN, RIGHT_MIDDLE, weight_left[RIGHT_MIDDLE].getV());
	set_link(WEIGHT, LAYER_HIDDEN, 0, LAYER_IN, RIGHT, weight_left[RIGHT].getV());
	set_link(WEIGHT, LAYER_HIDDEN, 0, LAYER_IN, RIGHT_BACK, weight_left[RIGHT_BACK].getV());

	set_link(WEIGHT, LAYER_HIDDEN, 0, LAYER_IN, LEFT_BACK, weight_left[LEFT_BACK].getV());
	set_link(WEIGHT, LAYER_HIDDEN, 0, LAYER_IN, LEFT, weight_left[LEFT].getV());
	set_link(WEIGHT, LAYER_HIDDEN, 0, LAYER_IN, LEFT_MIDDLE, weight_left[LEFT_MIDDLE].getV());
	set_link(WEIGHT, LAYER_HIDDEN, 0, LAYER_IN, LEFT_FRONT, weight_left[LEFT_FRONT].getV());

	set_link(WEIGHT, LAYER_HIDDEN, 1, LAYER_IN, RIGHT_FRONT, weight_right[RIGHT_FRONT].getV());
	set_link(WEIGHT, LAYER_HIDDEN, 1, LAYER_IN, RIGHT_MIDDLE, weight_right[RIGHT_MIDDLE].getV());
	set_link(WEIGHT, LAYER_HIDDEN, 1, LAYER_IN, RIGHT, weight_right[RIGHT].getV());
	set_link(WEIGHT, LAYER_HIDDEN, 1, LAYER_IN, RIGHT_BACK, weight_right[RIGHT_BACK].getV());
	set_link(WEIGHT, LAYER_HIDDEN, 1, LAYER_IN, LEFT_BACK, weight_right[LEFT_BACK].getV());
	set_link(WEIGHT, LAYER_HIDDEN, 1, LAYER_IN, LEFT, weight_right[LEFT].getV());
	set_link(WEIGHT, LAYER_HIDDEN, 1, LAYER_IN, LEFT_MIDDLE, weight_right[LEFT_MIDDLE].getV());
	set_link(WEIGHT, LAYER_HIDDEN, 1, LAYER_IN, LEFT_FRONT, weight_right[LEFT_FRONT].getV());

  set_link(WEIGHT, LAYER_OUT, 0, LAYER_HIDDEN, 0, 1.0);
  set_link(WEIGHT, LAYER_OUT, 1, LAYER_HIDDEN, 1, 1.0);
}

void NNet::Weights()
{
	  std_wl[RIGHT_FRONT].setV( -1.1 );
	  std_wl[RIGHT_MIDDLE].setV( -0.8 );
	  std_wl[RIGHT].setV( -0.1 );
	  std_wl[RIGHT_BACK].setV( 0 );
	  std_wl[LEFT_BACK].setV( 0 );
	  std_wl[LEFT].setV( 0.1 );
	  std_wl[LEFT_MIDDLE].setV( 0.8 );
	  std_wl[LEFT_FRONT].setV( -1.1 );

	  std_wr[RIGHT_FRONT].setV( -0.9 );
	  std_wr[RIGHT_MIDDLE].setV( 0.8 );
	  std_wr[RIGHT].setV( 0.1 );
	  std_wr[RIGHT_BACK].setV( 0 );
	  std_wr[LEFT_BACK].setV( 0 );
	  std_wr[LEFT].setV( -0.1 );
	  std_wr[LEFT_MIDDLE].setV( -0.8 );
	  std_wr[LEFT_FRONT].setV( -0.9 );

	for(int i = 0; i < NUM_SENS; i++)
	{
		weight_left[i].setV( std_wl[i].getV() );
		weight_right[i].setV( std_wr[i].getV() );
	}
}

Motor& NNet::output()
{
  double left, right;
  left = get_pattern(0, LAYER_OUT, 0) + OFFSET;
  right = get_pattern(0, LAYER_OUT, 1) + OFFSET;

  motor.move(  left, right );
  return motor;
}
